import pybullet as p
import time
import pybullet_data

from pybullet_utils import bullet_client
new_p = bullet_client.BulletClient(connection_mode=p.GUI)
physicsClientId = new_p._client
new_p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
# physicsClient = p.connect(p.GUI, options="--opengl2")  # or p.DIRECT for non-graphical version



p.setAdditionalSearchPath(pybullet_data.getDataPath())  # optionally
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF("r2d2.urdf", cubeStartPos, cubeStartOrientation)
for i in range(10000):
    p.stepSimulation()
    time.sleep(1./240.)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
print(cubePos, cubeOrn)
p.disconnect()
